import time, os, sys

from media.sensor import *
from media.display import *
from media.media import *

sensor_id = 2
sensor = None

# 显示模式选择：可以是 "VIRT"、"LCD" 或 "HDMI"
DISPLAY_MODE = "VIRT"

# 根据模式设置显示宽高
if DISPLAY_MODE == "VIRT":
    # 虚拟显示器模式
    DISPLAY_WIDTH = 800#ALIGN_UP(1920, 16)
    DISPLAY_HEIGHT = 480#1080
elif DISPLAY_MODE == "LCD":
    # 3.1寸屏幕模式
    DISPLAY_WIDTH = 800
    DISPLAY_HEIGHT = 480
elif DISPLAY_MODE == "HDMI":
    # HDMI扩展板模式
    DISPLAY_WIDTH = 1920
    DISPLAY_HEIGHT = 1080
else:
    raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

############################### 线的相关参数 ###############################
color_threshold = (86, 17, 56, 16, 0, 99) #红色阈值（阈值设置方法：阈值编辑器里在全黑下调整目标为白色）
roi_up            = [0,120,800,70]        #图片上方巡线颜色的查找区
roi_down            = [0,300,800,70]      #图片下方巡线颜色的查找区
up_centre_x = 0     # 上方色块的中心x轴位置
down_centre_x = 0   # 下方色块的中心x轴位置
line_dir = 0        # 线的方向

LINE_DIR_FORWARD = "FORWARD"    # 线往前
LINE__DIR_TO_LEFT = "LEFT"      # 线往左
LINE__DIR_TO_RIGHT = "RIGHT"    # 线往右

# 判断线的方向
# up=上方查找区的色块中心x轴
# down=下方查找区的色块中心x轴
# return 方向的英文字符串
def line_dir_status(up, down):
    error_range = 30 # 方向判断的误差范围
    up_down_difference_value = up - down   # 上下色块X轴的差值
    # 如果线往前
    if abs(up_down_difference_value) <= error_range:
        return LINE_DIR_FORWARD

    # 如果线往右
    if up_down_difference_value > 0:
        return LINE__DIR_TO_RIGHT

    # 如果线往左
    elif up_down_difference_value < 0:
        return LINE__DIR_TO_LEFT

try:
    # 构造一个具有默认配置的摄像头对象
    sensor = Sensor(id=sensor_id)
    # 重置摄像头sensor
    sensor.reset()

    # 无需进行镜像翻转
    # 设置水平镜像
    # sensor.set_hmirror(False)
    # 设置垂直翻转
    # sensor.set_vflip(False)

    # 设置通道0的输出尺寸
    sensor.set_framesize(width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, chn=CAM_CHN_ID_0)
    # 设置通道0的输出像素格式为RGB565
    sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)

    # 根据模式初始化显示器
    if DISPLAY_MODE == "VIRT":
        Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60)
    elif DISPLAY_MODE == "LCD":
        Display.init(Display.ST7701, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
    elif DISPLAY_MODE == "HDMI":
        Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)

    # 初始化媒体管理器
    MediaManager.init()
    # 启动传感器
    sensor.run()

    while True:
        os.exitpoint()

        # 捕获通道0的图像
        img = sensor.snapshot(chn=CAM_CHN_ID_0)

        # 查找两个区域中的目标线的颜色
        statistics_up = img.find_blobs([color_threshold],roi=roi_up,area_threshold=300,merge=True)
        statistics_down = img.find_blobs([color_threshold],roi=roi_down,area_threshold=300,merge=True)

        # 如果在图像上方识别到线的颜色
        if statistics_up:
            for blob in statistics_up:#遍历每一个色块
                # 根据色块的 xywh 绘制矩形
                img.draw_rectangle(blob[0:4], color=(255, 0, 0),thickness=2)
                # 根据色块的中心XY绘制十字线
                img.draw_cross(blob[5], blob[6])
                # 记录色块的中心x轴
                up_centre_x = blob[5]

        # 如果在图像下间识别到线的颜色
        if statistics_down:
            for blob in statistics_down:
                blob_xywh = blob[0:4]
                centre_x = blob[5]
                centre_y = blob[6]
                down_centre_x = blob[5]

                img.draw_rectangle(blob_xywh, color=(255, 0, 0),thickness=2)#画矩形
                img.draw_cross(centre_x, centre_y)#画十字

                # 打印色块位置信息
                string_start_x = 0
                string_start_y = 0
                font_size = 32
                string = "x={}, x - centre_x={}".format(centre_x, centre_x - (img.width() // 2))
                img.draw_string_advanced(string_start_x, string_start_y, font_size, string)

        # 打印方向信息
        line_dir = line_dir_status(up_centre_x, down_centre_x)
        string = "line_status={}".format(line_dir)
        img.draw_string_advanced(0, 480-32, 32, string)

        # 对两个查找区画矩形
        img.draw_rectangle(roi_up,color=(255, 255, 255),thickness=2)
        img.draw_rectangle(roi_down,color=(255, 255, 255),thickness=2)
        # 图像中心画十字
        img.draw_cross(img.width() // 2, img.height() // 2, color=(0, 255, 0), size=10, thickness=3)
        # 显示捕获的图像
        Display.show_image(img)

except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    # 停止传感器运行
    if isinstance(sensor, Sensor):
        sensor.stop()
    # 反初始化显示模块
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    # 释放媒体缓冲区
    MediaManager.deinit()
